#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Kd_tree.h"
#include <map>
#include <boost/foreach.hpp>

const int START_LABEL = 0;
const int END_LABEL = 1;

// Should be used with the Graph wrapper
struct Less_than_int
{
  bool operator()(const int& p1, 
                  const int& p2) const 
  {
    return (p1 < p2);
  }
};

class Prm
{
public:
    /*
        IMPORTANT:	The main code of the PRM algorithm should be placed in this file.
                    You may change its structure, but make sure you that it remains consistent
                    with the run() operation in Planner.h
    */


    Prm (	int number_vertices, 
            int k_nearest, 
            CollisionDetector* col, 
            Sampler* sampler,
            Point_2 start, Point_2 target) : 
        m_number_vertices(number_vertices), m_k_nearest(k_nearest), 
          m_col(col), m_sampler(sampler),m_graph(0),
          m_start(start), m_target(target)
    {
        //insert first the start and target points
        m_vertices.push_back(m_start);
        m_vertices.push_back(m_target);
        m_map[m_start] = START_LABEL;
        m_map[m_target] = END_LABEL;
        //initialize the Graph and kd-Tree
        m_graph = new Graph<int,Less_than_int>(0, true);
        m_kd_tree = new Kd_tree_2<Kernel>();
    }

    ~Prm()
    {
        delete m_graph;
        delete m_kd_tree;
    }
    
    //	This operation is supposed to generate the roadmap (sample configurations
    //	and connect them.
    void			generate_roadmap()
    {
        int last_index = END_LABEL;
        //Add m_number_vertices to the vector
        while (last_index < m_number_vertices + 2)
        {
            Point_2 p = m_sampler->generate_sample();
            ++last_index;
            m_vertices.push_back(p);
            //add each point to the map
            m_map[p] = last_index;
        }
        //add all points to the graph as vertices
        int index = 0;
        for (index = 0; index < m_vertices.size(); ++index)
        {
            m_graph->add_vertex(index);
        }

        //insert all points to the kd-tree
        m_kd_tree->insert(m_vertices.begin(), m_vertices.end());

        // go over all the points
        BOOST_FOREACH(Point_2& p, m_vertices){
            vector<Point_2>	nearest_neighbors;
            // find the k- nearest neighbors
            m_kd_tree->k_nearest_neighbors(p, m_k_nearest, std::back_inserter(nearest_neighbors));
            int p_index = m_map[p];
            BOOST_FOREACH(Point_2 q, nearest_neighbors){
                int q_index = m_map[q];
                // if there isn't an edge between them (or the connected components) and there is a valid-path (by the local planner) between them
                if (!m_graph->is_in_same_cc(p_index, q_index) && m_col->local_planner(p, q, m_col->m_radius))
                {
                    // add an edge to the graph
                    m_graph->add_edge(p_index, q_index);
                }
            }
        }

    }

    //	Returns a point path from start to target.
    //	If a path doesn't exist, returns empty vector.
    vector<Point_2>			retrieve_path()
    {
        vector<Point_2> generated_path;   // The generated path
        list<int> path;                   // The points indexes
        m_graph->find_path(START_LABEL, END_LABEL, path);

        list<int>::const_iterator iter;
        //for each point-index add the point to the generated_path
        for(iter = path.begin(); iter != path.end(); ++iter)
        {
            generated_path.insert(generated_path.end(), m_vertices[*iter]);
        }

        return generated_path;
    }

private:

    int									m_number_vertices;	// number of sampled vertices
    int									m_k_nearest;		// maximal degree of vertex in roadmap
    CollisionDetector*					m_col;
    Sampler*							m_sampler;
    
    Graph<int,Less_than_int>*			m_graph;			//	Graph structure
    Kd_tree_2<Kernel>*					m_kd_tree;			//	Kd-tree for nearest neighbor search

    Point_2 							m_start,m_target;	//	Start and target configurations

    vector<Point_2>                     m_vertices;         // Contains all the points
    map<Point_2, int>                   m_map;              // Maps between any point to it's index
};

#endif